/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "CTopoOccuMCL.h"
/// For reading the data from file

int readScanLine181(FILE *f,mcl::scan &meas, mcl::pose &p){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&p.x,&p.y,&p.a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}
int readLaserOdometryData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float p1,p2,p3;
		int i;
		char c;
		int cnt=0;
		fscanf(f,"%f %f %f %f %f %f ", &p.x,&p.y,&p.a,&p1,&p2,&p3); ///< Pose
		for(i=0;i<361;i++){
				if(i%2==0){
						fscanf(f,"%f ",&meas.r[cnt]);
						meas.a[i]=0.5*i*M_PI/180.0;
						cnt++;
						//fprintf(stderr,"%.2f ",meas.r[i]);
				} else fscanf(f,"%f ",&p1);
				
		}
		fprintf(stderr,"%d\n",cnt);
		meas.N = cnt;
		return 0;

}


void saveDPSLAM(FILE *f,mcl::scan &meas, mcl::pose &p){
	fprintf(f,"Odometry %f %f %f\n",p.x,p.y,p.a);
	fprintf(f,"Laser %d ",meas.N);
	for(int i = 0; i< meas.N; i++)  fprintf(f,"%.3f ",meas.r[i]); 
			
			fprintf(f,"\n");
}


int main(void){
		//FILE *log = fopen("j2b2_measurement.txt","rt");
		FILE *log = fopen("laser_update.txt","rt");
		if(log==NULL){
			fprintf(stderr,"No such file\n");	
			return 0;
		}
		FILE *log_save = fopen("pena_log.log","wt");
		if(log_save == NULL){
			fprintf(stderr,"Error opening file!\n");
			return 0;
		}
		
		mcl::scan s; ///< for occupancy grid
		s.allocate(181);
		mcl::pose odo_cur;
		while(!feof(log)){
				//readScanLine181(log,s,odo_cur);
				readLaserOdometryData(log,s,odo_cur);
				saveDPSLAM(log_save,s,odo_cur);
		}
		fclose(log); 
		fclose(log_save);
		fprintf(stderr,"Log file converted\n");
		return 0;
}